/*
 * Modbus.c
 *  Modbus RTU Master and Slave library for STM32 CUBE with FreeRTOS
 *  Created on: May 5, 2020
 *      Author: Alejandro Mera
 *      Adapted from https://github.com/smarmengol/Modbus-Master-Slave-for-Arduino
 */

#include "Modbus.h"
#include <stdlib.h>

/* -----------------Inner defines----------------- */
#ifndef __weak
#define __weak __attribute__((weak))
#endif

#define bitRead(value, bit) (((value) >> (bit)) & 0x01)
#define bitSet(value, bit) ((value) |= (1UL << (bit)))
#define bitClear(value, bit) ((value) &= ~(1UL << (bit)))
#define bitWrite(value, bit, bitvalue) ((bitvalue) ? bitSet(value, bit) : bitClear(value, bit))

#define lowByte(w) ((w)&0xff)
#define highByte(w) ((w) >> 8)
/* -----------------End of Inner defines----------------- */

/* -----------------Inner structure and type definitions----------------- */
/**
 * @enum MESSAGE
 * @brief
 * Indexes to telegram frame positions,数据帧中不同数据的位置标记
 */
typedef enum MESSAGE
{
	ID = 0,	 //!< ID field
	FUNC,	 //!< Function code position
	ADD_HI,	 //!< Address high byte
	ADD_LO,	 //!< Address low byte
	NB_HI,	 //!< Number of coils or registers high byte
	NB_LO,	 //!< Number of coils or registers low byte
	BYTE_CNT //!< byte counter
} mb_message_t;

// 部分数据大小定义
enum
{
	RESPONSE_SIZE = 6,
	EXCEPTION_SIZE = 3,
	CHECKSUM_SIZE = 2
};

// 请求队列
typedef struct
{
	modbus_t data[MAX_TELEGRAMS];
	uint8_t u8start;
	uint8_t u8end;
	uint16_t u16available;
	modbusHandler_t *MB_h;
} modbusTelegramQueue;
/* -----------------End of Inner structure and type definitions----------------- */

/* -----------------Inner variable declarations----------------- */
const unsigned char fctsupported[] =
	{
		MB_FC_READ_COILS,
		MB_FC_READ_DISCRETE_INPUT,
		MB_FC_READ_REGISTERS,
		MB_FC_READ_INPUT_REGISTER,
		MB_FC_WRITE_COIL,
		MB_FC_WRITE_REGISTER,
		MB_FC_WRITE_MULTIPLE_COILS,
		MB_FC_WRITE_MULTIPLE_REGISTERS};

static modbusTelegramQueue *modbusQueryQueue[MAX_M_HANDLERS] = {NULL};
static uint8_t numberQueryQueues = 0;
modbusHandler_t *mHandlers[MAX_M_HANDLERS] = {NULL};
uint8_t numberHandlers = 0;
/* -----------------End of Inner variable declarations----------------- */

/* -----------------Inner function declarations----------------- */
// Function prototypes for ModbusRingBuffer
static void RingAdd(modbusRingBuffer_t *xRingBuffer, uint8_t u8Val);							 // adds a byte to the ring buffer
static uint8_t RingGetAllBytes(modbusRingBuffer_t *xRingBuffer, uint8_t *buffer);				 // gets all the available bytes into buffer and return the number of bytes read
static uint8_t RingGetNBytes(modbusRingBuffer_t *xRingBuffer, uint8_t *buffer, uint16_t uNumber); // gets uNumber of bytes from ring buffer, returns the actual number of bytes read
static uint8_t RingCountBytes(modbusRingBuffer_t *xRingBuffer);									 // return the number of available bytes
static void RingClear(modbusRingBuffer_t *xRingBuffer);											 // flushes the ring buffer

// Fuctions slave
static int8_t process_FCx01(modbusHandler_t *modH);
static int8_t process_FCx03(modbusHandler_t *modH);
static int8_t process_FCx05(modbusHandler_t *modH);
static int8_t process_FCx06(modbusHandler_t *modH);
static int8_t process_FCx0f(modbusHandler_t *modH);
static int8_t process_FCx10(modbusHandler_t *modH);

// Fuctions master
static void get_FCx01(modbusHandler_t *modH);
static void get_FCx03(modbusHandler_t *modH);
static bool ModbusQueryPop(modbusHandler_t *modH, modbus_t *telegram);
static int8_t SendQuery(modbusHandler_t *modH, modbus_t telegram);

static void sendTxBuffer(modbusHandler_t *modH);
static int16_t getRxBuffer(modbusHandler_t *modH);
static void buildException(uint8_t u8exception, modbusHandler_t *modH);

// Fuctions misc
static int8_t validateAnswer(modbusHandler_t *modH);
static int8_t validateRequest(modbusHandler_t *modH);
static uint16_t calcCRC(uint8_t *Buffer, uint8_t u8length);
static uint16_t word(uint8_t H, uint8_t l);
/* -----------------END OF Inner function declarations----------------- */

/* -----------------User-oriented functions----------------- */
/**
 * @brief
 * Initialization for a Master/Slave.
 * this function will check the configuration parameters
 * of the modbus handler
 *
 * @param modH   modbus handler
 */
void ModbusReg(modbusHandler_t *modH)
{

	if (modH->u8id != 0 && modH->uModbusType == MB_MASTER)
	{
		while (1)
		{
			// error Master ID must be zero
		}
	}

	if (modH->u8id == 0 && modH->uModbusType == MB_SLAVE)
	{
		while (1)
		{
			// error Master ID must be zero
		}
	}
	if (numberHandlers < MAX_M_HANDLERS)
	{
		mHandlers[numberHandlers] = modH;
		numberHandlers++;
		if (modH->uModbusType == MB_MASTER)
		{
			modH->i8state = COM_IDLE;
			modbusQueryQueue[numberQueryQueues] = (modbusTelegramQueue *)malloc(sizeof(modbusTelegramQueue));
			modbusQueryQueue[numberQueryQueues]->MB_h = modH;
			ModbusQueryClean(modH);
			numberQueryQueues++;
		}
	}
	modH->u8BufferSize = 0;
	modH->u16InCnt = modH->u16OutCnt = modH->u16errCnt = 0;
}

void TaskModbusMaster(void *argument)
{
	modbusHandler_t *modH = (modbusHandler_t *)argument;

	if (modH->uModbusType != MB_MASTER)
	{
		modH->i8lastError = ERR_NOT_MASTER;
		modH->u16errCnt++;
		modbusErrorCallback(modH);
		return;
	}
	modbus_t telegram;
	switch (modH->i8state)
	{
	case COM_IDLE:
		if (!ModbusQueryPop(modH, &telegram))
		{
			break;
		}
		SendQuery(modH, telegram);
		modH->time_tick = 0;
		modH->i8state = COM_WAITING;
		break;
	case COM_WAITING:
		if (++(modH->time_tick) > TIMEOUT_MODBUS || modH->time_tick > modH->u16timeOut)
		{
			modH->i8lastError = ERR_TIME_OUT;
			modH->u16errCnt++;
			modH->i8state = COM_IDLE;
			modbusErrorCallback(modH);
		}
		break;
	case COM_PROCESSING:
		getRxBuffer(modH);
		if (modH->u8BufferSize < 6)
		{
			modH->i8lastError = ERR_BAD_SIZE;
			modH->u16errCnt++;
			modH->i8state = COM_IDLE;
			modbusErrorCallback(modH);
			break;
		}
		// validate message: id, CRC, FCT, exception
		int8_t u8exception = validateAnswer(modH);
		if (u8exception != 0)
		{
			modH->i8lastError = u8exception;
			modH->u16errCnt++;
			modH->i8state = COM_IDLE;
			modbusErrorCallback(modH);
			break;
		}
		modH->i8lastError = u8exception;

		// process answer
		switch (modH->u8Buffer[FUNC])
		{
		case MB_FC_READ_COILS:
		case MB_FC_READ_DISCRETE_INPUT:
			// call get_FCx01 to transfer the incoming message to u16regs buffer
			get_FCx01(modH);
			break;
		case MB_FC_READ_INPUT_REGISTER:
		case MB_FC_READ_REGISTERS:
			// call get_FCx03 to transfer the incoming message to u16regs buffer
			get_FCx03(modH);
			break;
		case MB_FC_WRITE_COIL:
		case MB_FC_WRITE_REGISTER:
		case MB_FC_WRITE_MULTIPLE_COILS:
		case MB_FC_WRITE_MULTIPLE_REGISTERS:
			// nothing to do
			break;
		default:
			break;
		}
		modH->i8state = COM_IDLE;
		break;
	default:
		break;
	}
}

void TaskModbusSlave(void *argument)
{

	modbusHandler_t *modH = (modbusHandler_t *)argument;
	// uint32_t notification;

	modH->i8lastError = 0;

	if (getRxBuffer(modH) == ERR_BUFF_OVERFLOW)
	{
		modH->i8lastError = ERR_BUFF_OVERFLOW;
		modH->u16errCnt++;
		modbusErrorCallback(modH);
		return;
	}

	if (modH->u8BufferSize < 7)
	{
		// The size of the frame is invalid
		modH->i8lastError = ERR_BAD_SIZE;
		modH->u16errCnt++;
		modbusErrorCallback(modH);
		return;
	}

	// check slave id
	if (modH->u8Buffer[ID] != modH->u8id)
	{
		return; // continue this is not for us
	}

	// validate message: CRC, FCT, address and size
	int8_t u8exception = validateRequest(modH);
	if (u8exception > 0)
	{
		if (u8exception != ERR_TIME_OUT)
		{
			buildException(u8exception, modH);
			sendTxBuffer(modH);
		}
		modH->i8lastError = u8exception;
		modH->u16errCnt++;
		modbusErrorCallback(modH);

		return;
	}

	modH->i8lastError = 0;

	// process message
	switch (modH->u8Buffer[FUNC])
	{
	case MB_FC_READ_COILS:
	case MB_FC_READ_DISCRETE_INPUT:
		modH->i8state = process_FCx01(modH);
		break;
	case MB_FC_READ_INPUT_REGISTER:
	case MB_FC_READ_REGISTERS:
		modH->i8state = process_FCx03(modH);
		break;
	case MB_FC_WRITE_COIL:
		modH->i8state = process_FCx05(modH);
		break;
	case MB_FC_WRITE_REGISTER:
		modH->i8state = process_FCx06(modH);
		break;
	case MB_FC_WRITE_MULTIPLE_COILS:
		modH->i8state = process_FCx0f(modH);
		break;
	case MB_FC_WRITE_MULTIPLE_REGISTERS:
		modH->i8state = process_FCx10(modH);
		break;
	default:
		break;
	}

	return;
}

__weak void TaskModbusMonitor(void *argument){
	#ifdef MONITOR_PRINT
	printf("Modbus:\r\n");

	if(numberHandlers==0){
		printf("No handler has been registered! \r\n");
		return;
	}
	for(int i=0;i<numberHandlers;i++){
		switch (mHandlers[i]->uModbusType)
		{
		case MB_SLAVE:
			printf("Slave ");
			break;
		case MB_MASTER:
			printf("Master ");
			break;
		default:
			printf("Unknow Type ");
			break;
		}
		printf("ID:0x%02X\r\n",mHandlers[i]->u8id);
		if(mHandlers[i]->u16errCnt!=0){
			printf(" Last Error Code: %d\r\n",mHandlers[i]->i8lastError);
		}
		printf(" state code: %d\r\n",mHandlers[i]->i8state);
		printf(" ERR count: %d\r\n",mHandlers[i]->u16errCnt);
		printf(" IN  count: %d\r\n",mHandlers[i]->u16InCnt);
		printf(" OUT count: %d\r\n",mHandlers[i]->u16OutCnt);
	}
	#endif
}

void ModbusQueryClean(modbusHandler_t *modH)
{
	if (numberQueryQueues == 0)
		return;

	for (int i = 0; i < numberQueryQueues; i++)
	{
		if (modbusQueryQueue[i]->MB_h == modH)
		{
			modbusQueryQueue[i]->u8start = 0;
			modbusQueryQueue[i]->u8end = 0;
			modbusQueryQueue[i]->u16available = 0;
			break;
		}
	}
}

bool ModbusQueryPush(modbusHandler_t *modH, modbus_t telegram)
{
	// Add the telegram to the TX tail Queue of Modbus Query

	if (numberQueryQueues == 0)
		return false;

	for (int i = 0; i < numberQueryQueues; i++)
	{
		if (modbusQueryQueue[i]->MB_h == modH)
		{
			modbusTelegramQueue *Queue = modbusQueryQueue[i];
			
			if (Queue->u16available < MAX_TELEGRAMS)
			{
				Queue->data[Queue->u8end] = telegram;
				Queue->u8end = (Queue->u8end + 1) % MAX_TELEGRAMS;
				Queue->u16available++;
				return true;
			}
			else
			{
				return false;
			}
		}
	}
	return false;
}

void ReceiveComplete(modbusHandler_t *modH)
{
	if (modH->i8state == COM_WAITING)
		modH->i8state = COM_PROCESSING;
}

__weak void ModbusUartSend(modbusHandler_t *modH, const uint8_t *data, uint8_t len)
{
	// 接口函数，用户在其他文件内自行实现
}

void ModbusUartRecv(modbusHandler_t *modH, uint8_t byte)
{
	RingAdd(&(modH->xBufferRX), byte);
}

__weak void modbusErrorCallback(modbusHandler_t* modH)
{
	// 报错回调函数，用户可以在其他文件内自行实现
	#ifdef MONITOR_PRINT
	switch (modH->uModbusType)
	{
	case MB_SLAVE:
		printf("Slave ");
		break;
	case MB_MASTER:
		printf("Master ");
		break;
	default:
		printf("Unknow Type ");
		break;
	}
	printf("ID:0x%02X\r\n",modH->u8id);
	if(modH->u16errCnt!=0){
		printf(" Last Error Code: %d\r\n",modH->i8lastError);
	}
	printf(" state code: %d\r\n",modH->i8state);
	printf(" ERR count: %d\r\n",modH->u16errCnt);
	printf(" IN  count: %d\r\n",modH->u16InCnt);
	printf(" OUT count: %d\r\n",modH->u16OutCnt);
	#endif
}
/* -----------------END of User-oriented functions----------------- */

/* -----------------Ring Buffer functions----------------- */
// This function must be called only after disabling USART RX interrupt or inside of the RX interrupt
void RingAdd(modbusRingBuffer_t *xRingBuffer, uint8_t u8Val)
{

	xRingBuffer->uxBuffer[xRingBuffer->u8end] = u8Val;
	xRingBuffer->u8end = (xRingBuffer->u8end + 1) % MAX_BUFFER;
	if (xRingBuffer->u16available == MAX_BUFFER)
	{
		xRingBuffer->overflow = true;
		xRingBuffer->u8start = (xRingBuffer->u8start + 1) % MAX_BUFFER;
	}
	else
	{
		xRingBuffer->overflow = false;
		xRingBuffer->u16available++;
	}
}

// This function must be called only after disabling USART RX interrupt
uint8_t RingGetAllBytes(modbusRingBuffer_t *xRingBuffer, uint8_t *buffer)
{
	return RingGetNBytes(xRingBuffer, buffer, xRingBuffer->u16available);
}

// This function must be called only after disabling USART RX interrupt
uint8_t RingGetNBytes(modbusRingBuffer_t *xRingBuffer, uint8_t *buffer, uint16_t uNumber)
{
	uint8_t uCounter;
	if (xRingBuffer->u16available == 0 || uNumber == 0)
		return 0;
	if (uNumber > MAX_BUFFER)
		return 0;

	for (uCounter = 0; uCounter < uNumber && uCounter < xRingBuffer->u16available; uCounter++)
	{
		buffer[uCounter] = xRingBuffer->uxBuffer[xRingBuffer->u8start];
		xRingBuffer->u8start = (xRingBuffer->u8start + 1) % MAX_BUFFER;
	}
	xRingBuffer->u16available = xRingBuffer->u16available - uCounter;
	xRingBuffer->overflow = false;
	RingClear(xRingBuffer);

	return uCounter;
}

uint8_t RingCountBytes(modbusRingBuffer_t *xRingBuffer)
{
	return xRingBuffer->u16available;
}

void RingClear(modbusRingBuffer_t *xRingBuffer)
{
	xRingBuffer->u8start = 0;
	xRingBuffer->u8end = 0;
	xRingBuffer->u16available = 0;
	xRingBuffer->overflow = false;
}

/* -----------------End of Ring Buffer functions----------------- */

/* -----------------Query functions----------------- */
bool ModbusQueryPop(modbusHandler_t *modH, modbus_t *telegram)
{

	if (numberQueryQueues == 0)
		return false;

	for (int i = 0; i < numberQueryQueues; i++)
	{
		if (modbusQueryQueue[i]->MB_h == modH)
		{
			modbusTelegramQueue *Queue = modbusQueryQueue[i];
			if (Queue->u16available == 0)
			{
				return false;
			}
			else
			{
				*telegram = Queue->data[Queue->u8start];
				Queue->u8start = (Queue->u8start + 1) % MAX_TELEGRAMS;
				Queue->u16available--;
				return true;
			}
		}
	}
	return false;
}

/**
 * @brief
 * *** Only Modbus Master ***
 * Generate a query to an slave with a modbus_t telegram structure
 * The Master must be in COM_IDLE mode. After it, its state would be COM_WAITING.
 * This method has to be called only in loop() section.
 *
 * @see modbus_t
 * @param modH  modbus handler
 * @param modbus_t  modbus telegram structure (id, fct, ...)
 * @ingroup loop
 */
int8_t SendQuery(modbusHandler_t *modH, modbus_t telegram)
{

	uint8_t u8regsno, u8bytesno;
	uint8_t error = 0;
	// xSemaphoreTake(modH->ModBusSphrHandle , portMAX_DELAY); //before processing the message get the semaphore

	if (modH->u8id != 0)
	{
		modH->i8lastError = error = ERR_NOT_MASTER;
		modH->u16errCnt++;
		modbusErrorCallback(modH);
	}
	if (modH->i8state != COM_IDLE)
	{
		modH->i8lastError = error = ERR_POLLING;
		modH->u16errCnt++;
		modbusErrorCallback(modH);
	}
	// if ((telegram.u8id == 0) || (telegram.u8id > 247))
	if ((telegram.u8id > 247))
	{
		modH->i8lastError = error = ERR_BAD_SLAVE_ID;
		modH->u16errCnt++;
		modbusErrorCallback(modH);
	}

	if (error)
	{
		return error;
	}

	modH->u16regs = telegram.u16reg;

	// telegram header
	modH->u8Buffer[ID] = telegram.u8id;
	modH->u8Buffer[FUNC] = telegram.u8fct;
	modH->u8Buffer[ADD_HI] = highByte(telegram.u16RegAdd);
	modH->u8Buffer[ADD_LO] = lowByte(telegram.u16RegAdd);

	switch (telegram.u8fct)
	{
	case MB_FC_READ_COILS:
	case MB_FC_READ_DISCRETE_INPUT:
	case MB_FC_READ_REGISTERS:
	case MB_FC_READ_INPUT_REGISTER:
		modH->u8Buffer[NB_HI] = highByte(telegram.u16CoilsNo);
		modH->u8Buffer[NB_LO] = lowByte(telegram.u16CoilsNo);
		modH->u8BufferSize = 6;
		break;
	case MB_FC_WRITE_COIL:
		modH->u8Buffer[NB_HI] = ((telegram.u16reg[0] > 0) ? 0xff : 0);
		modH->u8Buffer[NB_LO] = 0;
		modH->u8BufferSize = 6;
		break;
	case MB_FC_WRITE_REGISTER:
		modH->u8Buffer[NB_HI] = highByte(telegram.u16reg[0]);
		modH->u8Buffer[NB_LO] = lowByte(telegram.u16reg[0]);
		modH->u8BufferSize = 6;
		break;
	case MB_FC_WRITE_MULTIPLE_COILS: // TODO: implement "sending coils"
		u8regsno = telegram.u16CoilsNo / 16;
		u8bytesno = u8regsno * 2;
		if ((telegram.u16CoilsNo % 16) != 0)
		{
			u8bytesno++;
			u8regsno++;
		}

		modH->u8Buffer[NB_HI] = highByte(telegram.u16CoilsNo);
		modH->u8Buffer[NB_LO] = lowByte(telegram.u16CoilsNo);
		modH->u8Buffer[BYTE_CNT] = u8bytesno;
		modH->u8BufferSize = 7;

		for (uint16_t i = 0; i < u8bytesno; i++)
		{
			if (i % 2)
			{
				modH->u8Buffer[modH->u8BufferSize] = lowByte(telegram.u16reg[i / 2]);
			}
			else
			{
				modH->u8Buffer[modH->u8BufferSize] = highByte(telegram.u16reg[i / 2]);
			}
			modH->u8BufferSize++;
		}
		break;

	case MB_FC_WRITE_MULTIPLE_REGISTERS:
		modH->u8Buffer[NB_HI] = highByte(telegram.u16CoilsNo);
		modH->u8Buffer[NB_LO] = lowByte(telegram.u16CoilsNo);
		modH->u8Buffer[BYTE_CNT] = (uint8_t)(telegram.u16CoilsNo * 2);
		modH->u8BufferSize = 7;

		for (uint16_t i = 0; i < telegram.u16CoilsNo; i++)
		{

			modH->u8Buffer[modH->u8BufferSize] = highByte(telegram.u16reg[i]);
			modH->u8BufferSize++;
			modH->u8Buffer[modH->u8BufferSize] = lowByte(telegram.u16reg[i]);
			modH->u8BufferSize++;
		}
		break;
	}

	sendTxBuffer(modH);

	// xSemaphoreGive(modH->ModBusSphrHandle);

	modH->i8lastError = 0;
	return 0;
}
/* -----------------End of Query functions----------------- */

/* -----------------Tx&Rx Buffer functions----------------- */
/**
 * @brief
 * This method transmits u8Buffer to Serial line.
 * Only if u8txenpin != 0, there is a flow handling in order to keep
 * the RS485 transceiver in output state as long as the message is being sent.
 * This is done with TC bit.
 * The CRC is appended to the buffer before starting to send it.
 *
 * @return nothing
 * @ingroup modH Modbus handler
 */
void sendTxBuffer(modbusHandler_t *modH)
{
	// append CRC to message

	uint16_t u16crc = calcCRC(modH->u8Buffer, modH->u8BufferSize);
	modH->u8Buffer[modH->u8BufferSize] = u16crc >> 8;
	modH->u8BufferSize++;
	modH->u8Buffer[modH->u8BufferSize] = u16crc & 0x00ff;
	modH->u8BufferSize++;
	#ifdef SEND_MSG_PRINT
	printf("Send:");
	for(int i=0;i<modH->u8BufferSize;i++){
		printf("%02X ",modH->u8Buffer[i]);
	}
	printf("\r\n");
	#endif
	ModbusUartSend(modH, modH->u8Buffer, modH->u8BufferSize);
	modH->u8BufferSize = 0;
	// increase message counter
	modH->u16OutCnt++;
}

/**
 * @brief
 * This method moves Serial buffer data to the Modbus u8Buffer.
 *
 * @return buffer size if OK, ERR_BUFF_OVERFLOW if u8BufferSize >= MAX_BUFFER
 * @ingroup buffer
 */
int16_t getRxBuffer(modbusHandler_t *modH)
{

	int16_t i16result;

	if (modH->xBufferRX.overflow)
	{
		RingClear(&modH->xBufferRX); // clean up the overflowed buffer
		i16result = ERR_BUFF_OVERFLOW;
	}
	else
	{
		modH->u8BufferSize = RingGetAllBytes(&modH->xBufferRX, modH->u8Buffer);
		modH->u16InCnt++;
		i16result = modH->u8BufferSize;
		#ifdef RECV_MSG_PRINT
		printf("Recv:");
		for(int i=0;i<modH->u8BufferSize;i++){
			printf("%02X ",modH->u8Buffer[i]);
		}
		printf("\r\n");
		#endif
	}
	return i16result;
}
/* -----------------End of Tx&Rx Buffer functions----------------- */

/* -----------------Validate functions----------------- */
/**
 * @brief
 * This method validates slave incoming messages
 * 判断从机收到的数据是否有错误。
 *
 * @return 0 if OK, EXCEPTION if anything fails
 * @ingroup modH Modbus handler
 */
int8_t validateRequest(modbusHandler_t *modH)
{
	// check message crc vs calculated crc

	uint16_t u16MsgCRC;
	u16MsgCRC = ((modH->u8Buffer[modH->u8BufferSize - 2] << 8) | modH->u8Buffer[modH->u8BufferSize - 1]); // combine the crc Low & High bytes

	if (calcCRC(modH->u8Buffer, modH->u8BufferSize - 2) != u16MsgCRC)
	{
		modH->u16errCnt++;
		return ERR_BAD_CRC;
	}

	// check fct code
	bool isSupported = false;
	for (uint8_t i = 0; i < sizeof(fctsupported); i++)
	{
		if (fctsupported[i] == modH->u8Buffer[FUNC])
		{
			isSupported = 1;
			break;
		}
	}
	if (!isSupported)
	{
		modH->u16errCnt++;
		return EXC_FUNC_CODE;
	}

	// check start address & nb range
	uint16_t u16AdRegs = 0;
	uint16_t u16NRegs = 0;

	// uint8_t u8regs;
	switch (modH->u8Buffer[FUNC])
	{
	case MB_FC_READ_COILS:
	case MB_FC_READ_DISCRETE_INPUT:
	case MB_FC_WRITE_MULTIPLE_COILS:
		u16AdRegs = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]) / 16;
		u16NRegs = word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]) / 16;
		if (word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]) % 16)
			u16NRegs++; // check for incomplete words
		// verify address range
		if ((u16AdRegs + u16NRegs) > modH->u16regsize)
			return EXC_ADDR_RANGE;

		// verify answer frame size in bytes

		u16NRegs = word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]) / 8;
		if (word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]) % 8)
			u16NRegs++;
		u16NRegs = u16NRegs + 5; // adding the header  and CRC ( Slave address + Function code  + number of data bytes to follow + 2-byte CRC )
		if (u16NRegs > 256)
			return EXC_REGS_QUANT;

		break;
	case MB_FC_WRITE_COIL:
		u16AdRegs = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]) / 16;
		if (word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]) % 16)
			u16AdRegs++; // check for incomplete words
		if (u16AdRegs > modH->u16regsize)
			return EXC_ADDR_RANGE;
		break;
	case MB_FC_WRITE_REGISTER:
		u16AdRegs = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]);
		if (u16AdRegs > modH->u16regsize)
			return EXC_ADDR_RANGE;
		break;
	case MB_FC_READ_REGISTERS:
	case MB_FC_READ_INPUT_REGISTER:
	case MB_FC_WRITE_MULTIPLE_REGISTERS:
		u16AdRegs = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]);
		u16NRegs = word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]);
		if ((u16AdRegs + u16NRegs) > modH->u16regsize)
			return EXC_ADDR_RANGE;

		// verify answer frame size in bytes
		u16NRegs = u16NRegs * 2 + 5; // adding the header  and CRC
		if (u16NRegs > 256)
			return EXC_REGS_QUANT;
		break;
	}
	return 0; // OK, no exception code thrown
}

/**
 * @brief
 * This method validates master incoming messages
 *
 * @return 0 if OK, EXCEPTION if anything fails
 * @ingroup buffer
 */
int8_t validateAnswer(modbusHandler_t *modH)
{
	// check message crc vs calculated crc

#if ENABLE_TCP == 1
	if (modH->xTypeHW != TCP_HW)
	{
#endif
		uint16_t u16MsgCRC =
			((modH->u8Buffer[modH->u8BufferSize - 2] << 8) | modH->u8Buffer[modH->u8BufferSize - 1]); // combine the crc Low & High bytes
		if (calcCRC(modH->u8Buffer, modH->u8BufferSize - 2) != u16MsgCRC)
		{
			modH->u16errCnt++;
			return ERR_BAD_CRC;
		}
#if ENABLE_TCP == 1
	}
#endif

	// check exception
	if ((modH->u8Buffer[FUNC] & 0x80) != 0)
	{
		modH->u16errCnt++;
		return ERR_EXCEPTION;
	}

	// check fct code
	bool isSupported = false;
	for (uint8_t i = 0; i < sizeof(fctsupported); i++)
	{
		if (fctsupported[i] == modH->u8Buffer[FUNC])
		{
			isSupported = 1;
			break;
		}
	}
	if (!isSupported)
	{
		modH->u16errCnt++;
		return EXC_FUNC_CODE;
	}

	return 0; // OK, no exception code thrown
}
/* -----------------End of Validate functions----------------- */

/* -----------------Misc functions----------------- */
/**
 * @brief
 * This method creates a word from 2 bytes
 *
 * @return uint16_t (word)
 * @ingroup H  Most significant byte
 * @ingroup L  Less significant byte
 */
uint16_t word(uint8_t H, uint8_t L)
{
	bytesFields W;
	W.u8[0] = L;
	W.u8[1] = H;

	return W.u16[0];
}

/**
 * @brief
 * This method calculates CRC
 *
 * @return uint16_t calculated CRC value for the message
 * @ingroup Buffer
 * @ingroup u8length
 */
uint16_t calcCRC(uint8_t *Buffer, uint8_t u8length)
{
	unsigned int temp, temp2, flag;
	temp = 0xFFFF;
	for (unsigned char i = 0; i < u8length; i++)
	{
		temp = temp ^ Buffer[i];
		for (unsigned char j = 1; j <= 8; j++)
		{
			flag = temp & 0x0001;
			temp >>= 1;
			if (flag)
				temp ^= 0xA001;
		}
	}
	// Reverse byte order.
	temp2 = temp >> 8;
	temp = (temp << 8) | temp2;
	temp &= 0xFFFF;
	// the returned value is already swapped
	// crcLo byte is first & crcHi byte is last
	return temp;
}

/**
 * @brief
 * This method builds an exception message
 * 从机生成错误异常数据
 *
 * @ingroup u8exception exception number
 * @ingroup modH modbus handler
 */
void buildException(uint8_t u8exception, modbusHandler_t *modH)
{
	uint8_t u8func = modH->u8Buffer[FUNC]; // get the original FUNC code

	modH->u8Buffer[ID] = modH->u8id;
	modH->u8Buffer[FUNC] = u8func + 0x80;
	modH->u8Buffer[2] = u8exception;
	modH->u8BufferSize = EXCEPTION_SIZE;
}
/* -----------------End of Misc functions----------------- */

/* -----------------Frame process functions----------------- */
/**
 * This method processes functions 1 & 2 (for master)
 * This method puts the slave answer into master data buffer
 *
 * @ingroup register
 */
void get_FCx01(modbusHandler_t *modH)
{
	uint8_t u8byte, i;
	u8byte = 3;
	for (i = 0; i < modH->u8Buffer[2]; i++)
	{

		if (i % 2)
		{
			modH->u16regs[i / 2] = word(modH->u8Buffer[i + u8byte], lowByte(modH->u16regs[i / 2]));
		}
		else
		{

			modH->u16regs[i / 2] = word(highByte(modH->u16regs[i / 2]), modH->u8Buffer[i + u8byte]);
		}
	}
}

/**
 * This method processes functions 3 & 4 (for master)
 * This method puts the slave answer into master data buffer
 *
 * @ingroup register
 */
void get_FCx03(modbusHandler_t *modH)
{
	uint8_t u8byte, i;
	u8byte = 3;

	for (i = 0; i < modH->u8Buffer[2] / 2; i++)
	{
		modH->u16regs[i] = word(modH->u8Buffer[u8byte], modH->u8Buffer[u8byte + 1]);
		u8byte += 2;
	}
}

/**
 * @brief
 * This method processes functions 1 & 2
 * This method reads a bit array and transfers it to the master
 *
 * @return u8BufferSize Response to master length
 * @ingroup discrete
 */
int8_t process_FCx01(modbusHandler_t *modH)
{
	uint16_t u16currentRegister;
	uint8_t u8currentBit, u8bytesno, u8bitsno;
	uint8_t u8CopyBufferSize;
	uint16_t u16currentCoil, u16coil;

	// get the first and last coil from the message
	uint16_t u16StartCoil = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]);
	uint16_t u16Coilno = word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]);

	// put the number of bytes in the outcoming message
	u8bytesno = (uint8_t)(u16Coilno / 8);
	if (u16Coilno % 8 != 0)
		u8bytesno++;
	modH->u8Buffer[ADD_HI] = u8bytesno;
	modH->u8BufferSize = ADD_LO;
	modH->u8Buffer[modH->u8BufferSize + u8bytesno - 1] = 0;

	// read each coil from the register map and put its value inside the outcoming message
	u8bitsno = 0;

	for (u16currentCoil = 0; u16currentCoil < u16Coilno; u16currentCoil++)
	{
		u16coil = u16StartCoil + u16currentCoil;
		u16currentRegister = (u16coil / 16);
		u8currentBit = (uint8_t)(u16coil % 16);

		bitWrite(
			modH->u8Buffer[modH->u8BufferSize],
			u8bitsno,
			bitRead(modH->u16regs[u16currentRegister], u8currentBit));
		u8bitsno++;

		if (u8bitsno > 7)
		{
			u8bitsno = 0;
			modH->u8BufferSize++;
		}
	}

	// send outcoming message
	if (u16Coilno % 8 != 0)
		modH->u8BufferSize++;
	u8CopyBufferSize = modH->u8BufferSize + 2;
	sendTxBuffer(modH);
	return u8CopyBufferSize;
}

/**
 * @brief
 * This method processes functions 3 & 4
 * This method reads a word array and transfers it to the master
 *
 * @return u8BufferSize Response to master length
 * @ingroup register
 */
int8_t process_FCx03(modbusHandler_t *modH)
{

	uint16_t u16StartAdd = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]);
	uint8_t u8regsno = word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]);
	uint8_t u8CopyBufferSize;
	uint16_t i;

	modH->u8Buffer[2] = u8regsno * 2;
	modH->u8BufferSize = 3;

	for (i = u16StartAdd; i < u16StartAdd + u8regsno; i++)
	{
		modH->u8Buffer[modH->u8BufferSize] = highByte(modH->u16regs[i]);
		modH->u8BufferSize++;
		modH->u8Buffer[modH->u8BufferSize] = lowByte(modH->u16regs[i]);
		modH->u8BufferSize++;
	}
	u8CopyBufferSize = modH->u8BufferSize + 2;
	sendTxBuffer(modH);

	return u8CopyBufferSize;
}

/**
 * @brief
 * This method processes function 5
 * This method writes a value assigned by the master to a single bit
 *
 * @return u8BufferSize Response to master length
 * @ingroup discrete
 */
int8_t process_FCx05(modbusHandler_t *modH)
{
	uint8_t u8currentBit;
	uint16_t u16currentRegister;
	uint8_t u8CopyBufferSize;
	uint16_t u16coil = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]);

	// point to the register and its bit
	u16currentRegister = (u16coil / 16);
	u8currentBit = (uint8_t)(u16coil % 16);

	// write to coil
	bitWrite(
		modH->u16regs[u16currentRegister],
		u8currentBit,
		modH->u8Buffer[NB_HI] == 0xff);

	// send answer to master
	modH->u8BufferSize = 6;
	u8CopyBufferSize = modH->u8BufferSize + 2;
	sendTxBuffer(modH);

	return u8CopyBufferSize;
}

/**
 * @brief
 * This method processes function 6
 * This method writes a value assigned by the master to a single word
 *
 * @return u8BufferSize Response to master length
 * @ingroup register
 */
int8_t process_FCx06(modbusHandler_t *modH)
{

	uint16_t u16add = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]);
	uint8_t u8CopyBufferSize;
	uint16_t u16val = word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]);

	modH->u16regs[u16add] = u16val;

	// keep the same header
	modH->u8BufferSize = RESPONSE_SIZE;

	u8CopyBufferSize = modH->u8BufferSize + 2;
	sendTxBuffer(modH);

	return u8CopyBufferSize;
}

/**
 * @brief
 * This method processes function 15
 * This method writes a bit array assigned by the master
 *
 * @return u8BufferSize Response to master length
 * @ingroup discrete
 */
int8_t process_FCx0f(modbusHandler_t *modH)
{
	uint8_t u8currentBit, u8frameByte, u8bitsno;
	uint16_t u16currentRegister;
	uint8_t u8CopyBufferSize;
	uint16_t u16currentCoil, u16coil;
	bool bTemp;

	// get the first and last coil from the message
	uint16_t u16StartCoil = word(modH->u8Buffer[ADD_HI], modH->u8Buffer[ADD_LO]);
	uint16_t u16Coilno = word(modH->u8Buffer[NB_HI], modH->u8Buffer[NB_LO]);

	// read each coil from the register map and put its value inside the outcoming message
	u8bitsno = 0;
	u8frameByte = 7;
	for (u16currentCoil = 0; u16currentCoil < u16Coilno; u16currentCoil++)
	{

		u16coil = u16StartCoil + u16currentCoil;
		u16currentRegister = (u16coil / 16);
		u8currentBit = (uint8_t)(u16coil % 16);

		bTemp = bitRead(
			modH->u8Buffer[u8frameByte],
			u8bitsno);

		bitWrite(
			modH->u16regs[u16currentRegister],
			u8currentBit,
			bTemp);

		u8bitsno++;

		if (u8bitsno > 7)
		{
			u8bitsno = 0;
			u8frameByte++;
		}
	}

	// send outcoming message
	// it's just a copy of the incomping frame until 6th byte
	modH->u8BufferSize = 6;
	u8CopyBufferSize = modH->u8BufferSize + 2;
	sendTxBuffer(modH);
	return u8CopyBufferSize;
}

/**
 * @brief
 * This method processes function 16
 * This method writes a word array assigned by the master
 *
 * @return u8BufferSize Response to master length
 * @ingroup register
 */
int8_t process_FCx10(modbusHandler_t *modH)
{
	uint16_t u16StartAdd = modH->u8Buffer[ADD_HI] << 8 | modH->u8Buffer[ADD_LO];
	uint16_t u16regsno = modH->u8Buffer[NB_HI] << 8 | modH->u8Buffer[NB_LO];
	uint8_t u8CopyBufferSize;
	uint16_t i;
	uint16_t temp;

	// build header
	modH->u8Buffer[NB_HI] = 0;
	modH->u8Buffer[NB_LO] = (uint8_t)u16regsno; // answer is always 256 or less bytes
	modH->u8BufferSize = RESPONSE_SIZE;

	// write registers
	for (i = 0; i < u16regsno; i++)
	{
		temp = word(
			modH->u8Buffer[(BYTE_CNT + 1) + i * 2],
			modH->u8Buffer[(BYTE_CNT + 2) + i * 2]);

		modH->u16regs[u16StartAdd + i] = temp;
	}
	u8CopyBufferSize = modH->u8BufferSize + 2;
	sendTxBuffer(modH);

	return u8CopyBufferSize;
}
/* -----------------End of Frame process functions----------------- */